#include "ros/ros.h"
#include "tf/transform_listener.h"

#include <iostream>
#include <string>

#include <sensor_msgs/Imu.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/Header.h>
#include <std_msgs/String.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Float32MultiArray.h>

#include "mpccbf_line.h"
#include "mpccbf.h"
#include "get_dt35.h"

const int CTRL_IDLE = 1;
const int CTRL_INIT = 2;
const int CTRL_RUN = 3;

int ctrl_state = 0;

mpccbf::SystemState system_state;
mpccbf::TrajectoryPoint target_point;
std::vector<std::pair<double,double>> route_points;
std::vector<mpccbf::TrajectoryPoint> planning_published_trajectory;

sensor_msgs::Imu::ConstPtr g_imu_data;
float yaw_w = 0.0;
// void imuCallback(const sensor_msgs::Imu::ConstPtr& msg){

// }

void routeCallback(const std_msgs::Float32MultiArray& msg) {
    int size = msg.data.size() / 2;
    route_points.clear();
    for (int i = 0; i < size; ++i) {
        route_points.push_back(std::make_pair(msg.data[i * 2], msg.data[i * 2 + 1]));
    }
}

void odomCallback(const nav_msgs::Odometry::ConstPtr& msg){   //统一坐标系下
    system_state.v_x = -1 * msg->twist.twist.linear.y;
    system_state.v_y = msg->twist.twist.linear.x;
}

void poseCallback(const geometry_msgs::Pose::ConstPtr& msg) {
    system_state.x = msg->position.x;
    system_state.y = msg->position.y;
    tf::Quaternion q;
    tf::quaternionMsgToTF(msg->orientation,q);
    tf::Matrix3x3(q).getRPY(system_state.roll,system_state.pitch,system_state.yaw);
    system_state.yaw *= -1;
    system_state.heading = system_state.yaw;
}


void obstCallback(const std_msgs::Float32MultiArray& msg) {
    system_state.obs_x = msg.data[0];
    system_state.obs_y = msg.data[1];
}

void tgtCallback(const std_msgs::Float32MultiArray& msg) {
    if (msg.data.size() != 2) return;
    target_point.x = msg.data[0];
    target_point.y = msg.data[1];
}

void cmdCallback(const std_msgs::String& msg) {
    if (msg.data == "start_control") {
        if (ctrl_state == CTRL_IDLE) {
            ctrl_state = CTRL_INIT;
        }
    }
    else if (msg.data == "stop_control") {
        ctrl_state = CTRL_IDLE;
    }
} 

std::vector<uint8_t> get_bag(float v_x,float v_y,float w){
    std::vector<uint8_t> data(24);
    float p1 = 0.0;
    float p2 = 0.0;
    float p3 = 0.0;
    memcpy(data.data(), &v_x , 4);
    memcpy(data.data()+4, &v_y , 4);
    memcpy(data.data()+8, &w , 4);
    memcpy(data.data()+12, &p1 , 4);
    memcpy(data.data()+16, &p2 , 4);
    memcpy(data.data()+20, &p3 , 4);
    return data;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "mpccbf_node");
    ros::NodeHandle nh;
    ROS_INFO("Doing mpc-cbf");

    tf::TransformListener listener;
    Serial so;

    std::string str_o = "/dev/ttyUSB1";
    so.OpenSerial(str_o,E_BaudRate::_115200,E_DataSize::_8,E_Parity::None,E_StopBit::_1);
    ros::Subscriber sub_odom = nh.subscribe("/Odometry"    , 10 ,odomCallback);
    ros::Subscriber sub_pose = nh.subscribe("/location"    , 10 ,poseCallback);
    ros::Subscriber sub_cmd  = nh.subscribe("ctrl_cmd"     , 10 , cmdCallback);
    ros::Subscriber sub_tgt  = nh.subscribe("/mpc_target_point", 10 , tgtCallback);
    ros::Subscriber sub_route = nh.subscribe("/mpc_route"  , 10 , tgtCallback);
    // ros::Subscriber sub_obst  = n.subscribe("/polaris/obstacle", 10, obstCallback);

    mpccbf::CtrlCmd cmd;
    std::unique_ptr<mpccbf::MPCCBF> mpc_cbf = std::make_unique<mpccbf::MPCCBF>();

    ctrl_state = CTRL_IDLE;

    ros::Rate loop_rate(10);
    while(ros::ok()){
        ros::spinOnce();
        if(ctrl_state == CTRL_INIT){
            std::vector<double> headings;
            std::vector<double> accumulated_s;
            std::unique_ptr<mpccbf::ReferenceLine> reference_line = std::make_unique<mpccbf::ReferenceLine>(route_points);
            reference_line->ComputePathProfile(&headings,&accumulated_s);

            for(std::size_t i = 0; i<headings.size(); ++i){
                mpccbf::TrajectoryPoint trajectory_pt;
                trajectory_pt.x = route_points[i].first;
                trajectory_pt.y = route_points[i].second;
                trajectory_pt.heading = headings[i];
                trajectory_pt.v_x = 1.0;
                trajectory_pt.v_y = 1.0;
                planning_published_trajectory.push_back(trajectory_pt);
            }
            ctrl_state = CTRL_RUN;
        }
        else if(ctrl_state == CTRL_RUN){
            try{
                if(std::isnan(system_state.yaw)){
                    throw 0;
                }
                std::vector<double> result = mpc_cbf->Solve(system_state,planning_published_trajectory,target_point);
                if(result.size() < 3){
                    throw 0;
                }
                else {
                    cmd.steer_target = result[0];
                    cmd.acc_x = result[1];
                    cmd.acc_y = result[2];
                    double cost = result[3];
                    ROS_INFO("cmd.steer_omega: %f;", (float)cmd.steer_target);
                    ROS_INFO("cmd.v_x: %f;", (float)cmd.acc_x);
                    ROS_INFO("cmd.v_x: %f;", (float)cmd.acc_y);
                    ROS_INFO("cost: %f \n", (float)cost);
                }
            }
            catch(...){
                ROS_INFO("Error Occurred");
                cmd.acc_x = 0;
                cmd.acc_y = 0;
                cmd.steer_target = 0;
            }
            so.Send_to(get_bag(cmd.acc_x,cmd.acc_y,cmd.steer_target));
        }
    }

    so.Close();
    return 0;



}